#include <stdio.h>
#include <math.h>
#include "bsp_mlx90614.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/i2c.h"

static const char *TAG = "bsp_mlx90614";

// 从MLX90614读取温度数据 - 基于Adafruit库实现
static esp_err_t mlx90614_read_temp_register(bsp_mlx90614_handle_t *handle, uint8_t reg_addr, float *temperature) {
    if (handle == NULL || temperature == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    // 按照Adafruit库的实现方式
    // 使用 write_then_read 方法：发送寄存器地址，然后读取3字节数据
    
    uint8_t write_data[1] = {reg_addr}; // 只发送寄存器地址
    uint8_t read_data[3] = {0}; // 读取3字节：低字节、高字节、CRC
    esp_err_t ret;
    
    // 尝试多次读取，直到获得有效数据
    for (int attempt = 0; attempt < 3; attempt++) {
        // 使用 i2c_master_transmit_receive 实现 write_then_read
        ret = i2c_master_transmit_receive(handle->dev_handle, write_data, 1, read_data, 3, handle->config->timeout_ms);
        
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Failed to read temperature data for register 0x%02X (attempt %d)", reg_addr, attempt + 1);
            if (attempt < 2) {
                vTaskDelay(pdMS_TO_TICKS(100)); // 等待后重试
                continue;
            } else {
                *temperature = -999.0f; // 返回无效值
                return ret;
            }
        }
        
        // 强制显示原始数据用于调试
        // ESP_LOGI(TAG, "Register 0x%02X - Raw data: 0x%02X 0x%02X 0x%02X (attempt %d)", reg_addr, read_data[0], read_data[1], read_data[2], attempt + 1);
        
        // 检查数据是否有效 (0x0000 表示无效数据)
        if (read_data[0] == 0x00 && read_data[1] == 0x00) {
            ESP_LOGW(TAG, "Invalid temperature data received for register 0x%02X (attempt %d)", reg_addr, attempt + 1);
            if (attempt < 2) {
                vTaskDelay(pdMS_TO_TICKS(100)); // 等待更长时间再重试
                continue;
            } else {
                *temperature = -999.0f; // 返回无效值
                return ESP_ERR_INVALID_RESPONSE;
            }
        }
        
        // 数据有效，跳出重试循环
        break;
    }
    
    // 按照Adafruit库的方式转换温度数据
    // Adafruit库：temp *= 0.02; temp -= 273.15;
    uint16_t temp_raw = read_data[0] | (read_data[1] << 8);
    float temp = (float)temp_raw * 0.02f - 273.15f;
    *temperature = temp;
    
    // ESP_LOGI(TAG, "Register 0x%02X - Raw temp: 0x%04X, Converted: %.2f°C", reg_addr, temp_raw, temp);
    
    return ESP_OK;
}

esp_err_t bsp_mlx90614_init(i2c_master_bus_handle_t bus_handle, const bsp_mlx90614_config_t *config, bsp_mlx90614_handle_t *handle) {
    if (bus_handle == NULL || config == NULL || handle == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    // 初始化设备配置
    i2c_device_config_t dev_config = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = config->i2c_addr, // 使用7位地址 0x5A
        .scl_speed_hz = 100000,
    };
    
    // 添加设备到I2C总线
    esp_err_t ret = i2c_master_bus_add_device(bus_handle, &dev_config, &handle->dev_handle);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to add MLX90614 device to I2C bus");
        return ret;
    }
    
    // 保存句柄
    handle->bus_handle = bus_handle;
    handle->config = config;
    
    // 等待传感器稳定
    // ESP_LOGI(TAG, "Waiting for MLX90614 sensor to stabilize...");
    vTaskDelay(pdMS_TO_TICKS(100)); // 等待100ms让传感器稳定
    
    // 尝试读取传感器ID来验证连接
    uint8_t write_data[1] = {0x3C}; // ID1 寄存器
    uint8_t read_data[3] = {0};
    
    ret = i2c_master_transmit_receive(handle->dev_handle, write_data, 1, read_data, 3, config->timeout_ms);
    if (ret == ESP_OK) {
        uint16_t id = read_data[0] | (read_data[1] << 8);
        // ESP_LOGI(TAG, "MLX90614 sensor detected, ID: 0x%04X", id);
        
        // 如果ID是0xFFFF，尝试多次读取
        if (id == 0xFFFF) {
            // ESP_LOGW(TAG, "Received 0xFFFF, trying multiple reads...");
            for (int i = 0; i < 5; i++) {
                vTaskDelay(pdMS_TO_TICKS(100));
                ret = i2c_master_transmit_receive(handle->dev_handle, write_data, 1, read_data, 3, config->timeout_ms);
                if (ret == ESP_OK) {
                    id = read_data[0] | (read_data[1] << 8);
                    // ESP_LOGI(TAG, "Attempt %d - ID: 0x%04X", i+1, id);
                    if (id != 0xFFFF) break;
                }
            }
        }
    } else {
        // ESP_LOGW(TAG, "Could not communicate with sensor, but device added successfully");
    }
    
    // ESP_LOGI(TAG, "MLX90614 initialized successfully");
    return ESP_OK;
}

esp_err_t bsp_mlx90614_get_temperatures(bsp_mlx90614_handle_t *handle, float *ambient_temp, float *obj1_temp, float *obj2_temp) {
    if (handle == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    esp_err_t ret = ESP_OK;
    
    // 读取环境温度
    if (ambient_temp != NULL) {
        ret = mlx90614_read_temp_register(handle, MLX90614_REG_TA, ambient_temp);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Failed to read ambient temperature");
            return ret;
        }
    }
    
    // 读取物体温度1
    if (obj1_temp != NULL) {
        ret = mlx90614_read_temp_register(handle, MLX90614_REG_TOBJ1, obj1_temp);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Failed to read object temperature 1");
            return ret;
        }
    }
    
    // 读取物体温度2
    if (obj2_temp != NULL) {
        ret = mlx90614_read_temp_register(handle, MLX90614_REG_TOBJ2, obj2_temp);
        if (ret != ESP_OK) {
            ESP_LOGE(TAG, "Failed to read object temperature 2");
            return ret;
        }
    }
    
    return ESP_OK;
}

esp_err_t bsp_mlx90614_read_ambient_temp(bsp_mlx90614_handle_t *handle, float *temperature) {
    if (handle == NULL || temperature == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    return mlx90614_read_temp_register(handle, MLX90614_REG_TA, temperature);
}

esp_err_t bsp_mlx90614_read_object_temp(bsp_mlx90614_handle_t *handle, float *temperature) {
    if (handle == NULL || temperature == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    return mlx90614_read_temp_register(handle, MLX90614_REG_TOBJ1, temperature);
}

esp_err_t bsp_mlx90614_delete(bsp_mlx90614_handle_t *handle) {
    if (handle == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    esp_err_t ret = i2c_master_bus_rm_device(handle->dev_handle);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to remove MLX90614 device from I2C bus");
        return ret;
    }
    
    // ESP_LOGI(TAG, "MLX90614 device deleted");
    return ESP_OK;
}